/*******************************************************************************
*
* (c) Copyright 2014 Freescale Semiconductor
*
****************************************************************************//*!
*
* @file     TaskScheduler.c
*
* @author   Salvador Zendejas
*
* @version  0.0.1
*
* @date     Aug 22, 2014
*
* @brief    All the functions that allow to create, block or terminate Tasks and the Dispatcher
*
*******************************************************************************/

/*******************************************************************************
* Includes
*******************************************************************************/
#include "TaskScheduler.h" /* Include TaskScheduler declarations */

/*******************************************************************************
* Global variables
*******************************************************************************/
uint32_t u32StackAllocated[1024];
uint32_t *u32StackAllocatedptr;
uint32_t u32_gStackPtrReg;
uint32_t u32_gR5reg;


/*******************************************************************************
* Local variables
*******************************************************************************/


/*******************************************************************************
* Constants
*******************************************************************************/


/*******************************************************************************
* Global functions
*******************************************************************************/
void vfn_TaskScheduler_Init(void){
	/*assign the ptr of Stack Allocated to the reserved Array for the Stack of our tasks*/
	u32StackAllocatedptr = u32StackAllocated;
	ReadREG(u32_gStackPtrReg, "sp");
	ReadREG(u32_gR5reg, "r7");
	vfn_SysTick_INIT();
	
}

void vfn_TaskScheduler_Start(void){
	//vfn_SysTick_ISR_Cfg(TICKs_10MS);
	vfn_SysTick_ISR_Cfg(1200000);
}

void vfn_SysTick_INIT(void){
	/*Enabling the Module and Setting the Clk Source*/
	SYST_CSR |= (SET << SysTick_CSR_ENABLE_SHIFT) | (SET << SysTick_CSR_CLKSOURCE_SHIFT);
}

void vfn_SysTick_ISR_Cfg(uint32_t u32ReloadValue){
	SYST_CSR &= ~(SysTick_CSR_TICKINT_MASK);	//Disabling the ISR
	SYST_RVR = SysTick_RVR_RELOAD(u32ReloadValue);	//Charge the Value 
	SYST_CSR |= (SysTick_CSR_TICKINT_MASK);		//Enabling the ISR
}

/*******************************************************************************
*
* Function: eStatusFn vfn_TaskCreate(
*           (
*               uint32_t    		u32AddressFn,
*               eID_Task_t          ID_Task,
*               uint16_t  			StackSize,
*               uint8_t             *u8NameFn
*           )
*
* Description:  The TaskScheduler receive the notification to create a new Task, Check if not want to create more
* 			 task that the MAX_TASK_LOAD allows and then create the fake context and positioning the u32StackAllocatedptr
* 			 to the next stack segment fot the next task. The return value indicate if the Task was created or not.
*
*******************************************************************************/
eStatusFn vfn_TaskCreate(uint32_t *u32AddressFn, eID_Task_t ID_Task, uint16_t StackSize, uint8_t *u8NameFn){
	uint8_t u8_nameIndex = 0;	/*Index for save the Name on the array*/
	if (u8_IndexTaskObj == MAX_TASK_LOAD) return _Error;
	/*Creating the Fake Context*/
	t_TaskObjArr[u8_IndexTaskObj].u32_StackAddr = (uint32_t)(u32StackAllocatedptr);	//SP
	(*u32StackAllocatedptr++) = (uint32_t)(u32StackAllocatedptr + STKALIGN);	//R7
	(*u32StackAllocatedptr++) = (uint32_t)0xFFFFFFF9;	//EXC_RETURN
	(*u32StackAllocatedptr++) = (uint32_t)0xFE0BEBE;	//R0
	(*u32StackAllocatedptr++) = (uint32_t)0;	//R1
	(*u32StackAllocatedptr++) = (uint32_t)0;	//R2
	(*u32StackAllocatedptr++) = (uint32_t)0;	//R3
	(*u32StackAllocatedptr++) = (uint32_t)0;	//R12
	(*u32StackAllocatedptr++) = (uint32_t)0;	//LR
	t_TaskObjArr[u8_IndexTaskObj].t_ptrFn = u32AddressFn;
	(*u32StackAllocatedptr++) = (uint32_t)u32AddressFn;		//PC
	(*u32StackAllocatedptr++) = (uint32_t)(SET<<24);	//xPSR
	
	u32StackAllocatedptr += (uint32_t)(StackSize - 10); //Going to the next Stack segment for the next Task
	/*Saving the Params of the Task Object*/
	t_TaskObjArr[u8_IndexTaskObj].e_IDTask = ID_Task;
	/* Saving the Name of the Function*/
	for (u8_nameIndex; u8_nameIndex < NAME_LENGHT_MAX; u8_nameIndex++) {
		t_TaskObjArr[u8_IndexTaskObj].u8_Name[u8_nameIndex] = *u8NameFn++;
		if(*u8NameFn == '\0')	break;
	}
	t_TaskObjArr[u8_IndexTaskObj].u8_Priority = Priority_DEFAULT;
	t_TaskObjArr[u8_IndexTaskObj++].t_Status = _Blocked;
	return _OK;
}

void vfn_TaskActivate(eID_Task_t ID_Task){
	uint8_t u8indexTask = 0;
	for (u8indexTask = 0; u8indexTask < MAX_TASK_LOAD; u8indexTask++) {
		if (t_TaskObjArr[u8indexTask].e_IDTask == ID_Task)	t_TaskObjArr[u8indexTask].t_Status = _Ready;
	}
}

void vfn_TaskTerminate(eID_Task_t ID_Task){
	uint8_t u8indexTask = 0;
	for (u8indexTask = 0; u8indexTask < MAX_TASK_LOAD; u8indexTask++) {
		if (t_TaskObjArr[u8indexTask].e_IDTask == ID_Task)	t_TaskObjArr[u8indexTask].t_Status = _Terminate;
	}
}

void vfn_TaskDispatcher(void){
	uint8_t u8indexTask;
	
	if (u8_gActualTaskRunning == (MAX_TASK_LOAD-1))	u8indexTask = 0;
	else
		u8indexTask = u8_gActualTaskRunning + 1;
	
	do {
		if (t_TaskObjArr[u8indexTask].t_Status == _Ready && u8_gActualTaskRunning != u8indexTask){
			t_TaskObjArr[u8indexTask].t_Status == _Running;
			t_TaskObjArr[u8_gActualTaskRunning].t_Status == _Ready;
			u8_gActualTaskRunning = u8indexTask;
			break;
		}
		if (u8indexTask == (MAX_TASK_LOAD-1))	u8indexTask = 0;
		else
			u8indexTask++;
	} while (u8_gActualTaskRunning != u8indexTask);
	
}


/*******************************************************************************
* Local functions (static)
*******************************************************************************/


/*******************************************************************************
* ISR functions
*******************************************************************************/
void SysTick_Handler(void){
	t_TaskObjArr[u8_gActualTaskRunning].u32_StackAddr = u32_gR5reg;
	vfn_TaskDispatcher();
}
